/**
 * @file ArmorTrack.cpp
 * @brief 装甲板追踪类
 * @date 2021-09-12
 *
 */
#include "ArmorTrack.h"
#include <Eigen/Core>
#include <opencv2/core/eigen.hpp>

/**
 * @brief 装甲构造函数(默认初始化)
 * @param first_armor
 */
ArmorTracker::ArmorTracker(Features_ptr &first_armor) {
    // 记录首帧装甲板
    if (first_armor == nullptr) {
        return;
    }
    this->features_sequence.emplace_front(first_armor);
    // 两帧之间 img_delta_dis 初始化为 first_armor 的 x, y
    this->img_delta_dis = getDistance(first_armor->getCenter(), Point2f(0, 0));
    // 装甲板水平位变初始化
    this->x_change = 0.f;
    //卡尔曼初始化
    this->KF_Armor = KalmanFilterX<4, 4>(kalmanFilterParam.GYRO_PROCESS_ERROR, kalmanFilterParam.GYRO_MEASURE_ERROR);
    this->KF_Armor.setH(cv::Matx<float, 4, 4>::eye());
}


/**
 * @brief 装甲板序列更新
 * @param trackedArmor 此帧中被匹配的装甲板
 */
void ArmorTracker::update(const Features_ptr &trackedArmor) {
    //若为空则记录消失帧数+1，不加入追踪序列中
    if (trackedArmor == nullptr) {
        vanish_num++;
        return;
    } else {
        vanish_num = 0;
        trackedArmor->setMatchMessage(true);
        // 用于计算匹配误差
        // 更新 delta_dis
        this->img_delta_dis = getDistance(this->getGoalFeature()->getCenter(), trackedArmor->getCenter());
        // 更新 x_change
        this->x_change = trackedArmor->getCenter().x - this->getGoalFeature()->getCenter().x;
    }
    // 保持追踪序列最大长度
    if (this->features_sequence.size() >= (size_t) A_param.Seq_maxlength) {
        // 从后 pop
        this->features_sequence.pop_back();
    }
    // 从前 push
    this->features_sequence.push_front(trackedArmor);
}

/**
 * @brief 为匹配判断是否发生较大位置突变
 * @param to_be_matched 待匹配进入序列的特征
 * @param min_dis 最小像素间距
 * @return true表示位移过大，需新建序列
 */
bool ArmorTracker::isSuddenChange(Features_ptr &to_be_matched, float &min_dis) {
    // 获取当前帧图像中 x 方向速度
//    float new_x_speed = to_be_matched->getCenter().x - this->getGoalFeature()->getCenter().x;
//    if ((this->getGoalFeature()->getAngle() * to_be_matched->getAngle() < -80.f) ||
//        abs(new_x_speed / (this->x_change + 0.001f)) >
//        A_param.MAX_DIRECTION_RATIO) {
//        return true;
//    }
    if (this->getGoalFeature()->getAngle() * to_be_matched->getAngle() < -80.f) return true;
    if ((min_dis > A_param.MAX_DELTA_DIS) && (min_dis / this->img_delta_dis) > A_param.MAX_DELTA_DIS_RATIO) return true;
    else return false;
}